#ifndef XV_QUAT_OPS_H
#define XV_QUAT_OPS_H

#include <Eigen/Eigen>

namespace xvslam {

/**
 * equation 6 http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf
 */
inline Eigen::Matrix<double, 3, 3> skew_x(const Eigen::Matrix<double, 3, 1> &w) {
    Eigen::Matrix<double, 3, 3> w_x;
    w_x << 0, -w(2), w(1), w(2), 0, -w(0), -w(1), w(0), 0;
    return w_x;
}

inline Eigen::Matrix<double, 3, 1> vee(const Eigen::Matrix<double, 3, 3> &w_x) {
    Eigen::Matrix<double, 3, 1> w;
    w << w_x(2, 1), w_x(0, 2), w_x(1, 0);
    return w;
}

/**
 * equation 15. http://ethaneade.com/lie.pdf
 */
inline Eigen::Matrix<double, 3, 3> exp_so3(const Eigen::Matrix<double, 3, 1> &w) {
    // get theta
    Eigen::Matrix<double, 3, 3> w_x = skew_x(w);
    double theta = w.norm();
    // Handle small angle values
    double A, B;
    if (theta < 1e-7) {
        A = 1;
        B = 0.5;
    } else {
        A = sin(theta) / theta;
        B = (1 - cos(theta)) / (theta * theta);
    }
    // compute so(3) rotation
    Eigen::Matrix<double, 3, 3> R;
    if (theta == 0) {
        R = Eigen::MatrixXd::Identity(3, 3);
    } else {
        R = Eigen::MatrixXd::Identity(3, 3) + A * w_x + B * w_x * w_x;
    }
    return R;
}

/**
 * equation 17 & 18. http://ethaneade.com/lie.pdf
 * @param[in] R 3x3 SO(3) rotation matrix
 * @return 3x1 in the R(3) space [omegax, omegay, omegaz]
 */
inline Eigen::Matrix<double, 3, 1> log_so3(const Eigen::Matrix<double, 3, 3> &R) {

    // note switch to base 1
    double R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2);
    double R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2);
    double R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2);

    // Get trace(R)
    const double tr = R.trace();
    Eigen::Vector3d omega;

    // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
    // we do something special
    if (tr + 1.0 < 1e-10) {
        if (std::abs(R33 + 1.0) > 1e-5) {
            omega = (M_PI / sqrt(2.0 + 2.0 * R33)) * Eigen::Vector3d(R13, R23, 1.0 + R33);
        } else if (std::abs(R22 + 1.0) > 1e-5) {
            omega = (M_PI / sqrt(2.0 + 2.0 * R22)) * Eigen::Vector3d(R12, 1.0 + R22, R32);
        } else {
            // if(std::abs(R.r1_.x()+1.0) > 1e-5)  This is implicit
            omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Eigen::Vector3d(1.0 + R11, R21, R31);
        }
    } else {
        double magnitude;
        const double tr_3 = tr - 3.0; // always negative
        if (tr_3 < -1e-7) {
            double theta = acos((tr - 1.0) / 2.0);
            magnitude = theta / (2.0 * sin(theta));
        } else {
            // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
            // use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2)
            // see https://github.com/borglab/gtsam/issues/746 for details
            magnitude = 0.5 - tr_3 / 12.0;
        }
        omega = magnitude * Eigen::Vector3d(R32 - R23, R13 - R31, R21 - R12);
    }
    return omega;
}

/**
 * Equation 84 http://ethaneade.com/lie.pdf
 */
inline Eigen::Matrix4d exp_se3(Eigen::Matrix<double, 6, 1> vec) {

    // Precompute our values
    Eigen::Vector3d w = vec.head(3);
    Eigen::Vector3d u = vec.tail(3);
    double theta = sqrt(w.dot(w));
    Eigen::Matrix3d wskew;
    wskew << 0, -w(2), w(1), w(2), 0, -w(0), -w(1), w(0), 0;
  
    // Handle small angle values
    double A, B, C;
    if (theta < 1e-7) {
        A = 1;
        B = 0.5;
        C = 1.0 / 6.0;
    } else {
        A = sin(theta) / theta;
        B = (1 - cos(theta)) / (theta * theta);
        C = (1 - A) / (theta * theta);
    }
  
    // Matrices we need V and Identity
    Eigen::Matrix3d I_33 = Eigen::Matrix3d::Identity();
    Eigen::Matrix3d V = I_33 + B * wskew + C * wskew * wskew;
  
    // Get the final matrix to return
    Eigen::Matrix4d mat = Eigen::Matrix4d::Zero();
    mat.block(0, 0, 3, 3) = I_33 + A * wskew + B * wskew * wskew;
    mat.block(0, 3, 3, 1) = V * u;
    mat(3, 3) = 1;
    return mat;
}

/**
 * @brief SE(3) matrix logarithm
 *
 * Equation is from Ethan Eade's reference: http://ethaneade.com/lie.pdf
 * \f{align*}{
 * \boldsymbol\omega &=\mathrm{skew\_offdiags}\Big(\frac{\theta}{2\sin\theta}(\mathbf R - \mathbf R^\top)\Big) \\
 * \mathbf u &= \mathbf V^{-1}\mathbf t
 * \f}
 * where we have the following definitions
 * \f{align*}{
 * \theta &= \mathrm{arccos}((\mathrm{tr}(\mathbf R)-1)/2) \\
 * \mathbf V^{-1} &= \mathbf I - \frac{1}{2} \lfloor \boldsymbol\omega \times\rfloor + \frac{1}{\theta^2}\Big(1-\frac{A}{2B}\Big)\lfloor
 * \boldsymbol\omega \times\rfloor^2 \f}
 *
 * This function is based on the GTSAM one as the original implementation was a bit unstable.
 * See the following:
 * - https://github.com/borglab/gtsam/
 * - https://github.com/borglab/gtsam/issues/746
 * - https://github.com/borglab/gtsam/pull/780
 *
 * @param mat 4x4 SE(3) matrix
 * @return 6x1 in the R(6) space [omega, u]
 */
inline Eigen::Matrix<double, 6, 1> log_se3(Eigen::Matrix4d mat) {
    Eigen::Vector3d w = log_so3(mat.block<3, 3>(0, 0));
    Eigen::Vector3d T = mat.block<3, 1>(0, 3);
    const double t = w.norm();
    if (t < 1e-10) {
        Eigen::Matrix<double, 6, 1> log;
        log << w, T;
        return log;
    } else {
        Eigen::Matrix3d W = skew_x(w / t);
        // Formula from Agrawal06iros, equation (14)
        // simplified with Mathematica, and multiplying in T to avoid matrix math
        double Tan = tan(0.5 * t);
        Eigen::Vector3d WT = W * T;
        Eigen::Vector3d u = T - (0.5 * t) * WT + (1 - t / (2. * Tan)) * (W * WT);
        Eigen::Matrix<double, 6, 1> log;
        log << w, u;
        return log;
    }
}
  
/**
 * @brief Hat operator for R^6 -> Lie Algebra se(3)
 *
 * \f{align*}{
 * \boldsymbol\Omega^{\wedge} = \begin{bmatrix} \lfloor \boldsymbol\omega \times\rfloor & \mathbf u \\ \mathbf 0 & 0 \end{bmatrix}
 * \f}
 *
 * @param vec 6x1 in the R(6) space [omega, u]
 * @return Lie algebra se(3) 4x4 matrix
 */
inline Eigen::Matrix4d hat_se3(const Eigen::Matrix<double, 6, 1> &vec) {
    Eigen::Matrix4d mat = Eigen::Matrix4d::Zero();
    mat.block(0, 0, 3, 3) = skew_x(vec.head(3));
    mat.block(0, 3, 3, 1) = vec.tail(3);
    return mat;
}

/**
 * @brief SE(3) matrix analytical inverse
 *
 * It seems that using the .inverse() function is not a good way.
 * This should be used in all cases we need the inverse instead of numerical inverse.
 * https://github.com/rpng/open_vins/issues/12
 * \f{align*}{
 * \mathbf{T}^{-1} = \begin{bmatrix} \mathbf{R}^\top & -\mathbf{R}^\top\mathbf{p} \\ \mathbf{0} & 1 \end{bmatrix}
 * \f}
 *
 * @param[in] T SE(3) matrix
 * @return inversed SE(3) matrix
 */
inline Eigen::Matrix4d Inv_se3(const Eigen::Matrix4d &T) {
    Eigen::Matrix4d Tinv = Eigen::Matrix4d::Identity();
    Tinv.block(0, 0, 3, 3) = T.block(0, 0, 3, 3).transpose();
    Tinv.block(0, 3, 3, 1) = -Tinv.block(0, 0, 3, 3) * T.block(0, 3, 3, 1);
    return Tinv;
}

/**
 * @brief Gets roll, pitch, yaw of argument rotation (in that order).
 *
 * To recover the matrix: R_input = R_z(yaw)*R_y(pitch)*R_x(roll)
 * If you are interested in how to compute Jacobians checkout this report:
 * http://mars.cs.umn.edu/tr/reports/Trawny05c.pdf
 *
 * @param rot SO(3) rotation matrix
 * @return roll, pitch, yaw values (in that order)
 */
inline Eigen::Matrix<double, 3, 1> rot2rpy(const Eigen::Matrix<double, 3, 3> &rot) {
    Eigen::Matrix<double, 3, 1> rpy;
    rpy(1, 0) = atan2(-rot(2, 0), sqrt(rot(0, 0) * rot(0, 0) + rot(1, 0) * rot(1, 0)));
    if (std::abs(cos(rpy(1, 0))) > 1.0e-12) {
        rpy(2, 0) = atan2(rot(1, 0) / cos(rpy(1, 0)), rot(0, 0) / cos(rpy(1, 0)));
        rpy(0, 0) = atan2(rot(2, 1) / cos(rpy(1, 0)), rot(2, 2) / cos(rpy(1, 0)));
    } else {
        rpy(2, 0) = 0;
        rpy(0, 0) = atan2(rot(0, 1), rot(1, 1));
    }
    return rpy;
}
  
/**
 * @brief Construct rotation matrix from given roll
 * @param t roll angle
 * @return SO(3) rotation matrix
 */
inline Eigen::Matrix<double, 3, 3> rot_x(double t) {
    Eigen::Matrix<double, 3, 3> r;
    double ct = cos(t);
    double st = sin(t);
    r << 1.0, 0.0, 0.0, 0.0, ct, -st, 0.0, st, ct;
    return r;
}
  
/**
 * @brief Construct rotation matrix from given pitch
 * @param t pitch angle
 * @return SO(3) rotation matrix
 */
inline Eigen::Matrix<double, 3, 3> rot_y(double t) {
    Eigen::Matrix<double, 3, 3> r;
    double ct = cos(t);
    double st = sin(t);
    r << ct, 0.0, st, 0.0, 1.0, 0.0, -st, 0.0, ct;
    return r;
}
  
/**
 * @brief Construct rotation matrix from given yaw
 * @param t yaw angle
 * @return SO(3) rotation matrix
 */
inline Eigen::Matrix<double, 3, 3> rot_z(double t) {
    Eigen::Matrix<double, 3, 3> r;
    double ct = cos(t);
    double st = sin(t);
    r << ct, -st, 0.0, st, ct, 0.0, 0.0, 0.0, 1.0;
    return r;
}

/**
 * @brief Integrated quaternion from angular velocity
 *
 * See equation (48) of trawny tech report [Indirect Kalman Filter for 3D Attitude
 * Estimation](http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf).
 * This matrix is derived in Section 1.5 of the report by finding the Quaterion Time Derivative.
 *
 * \f{align*}{
 * \boldsymbol{\Omega}(\boldsymbol{\omega}) &=
 * \begin{bmatrix}
 * -\lfloor{\boldsymbol{\omega}}  \rfloor & \boldsymbol{\omega} \\
 * -\boldsymbol{\omega}^\top & 0
 * \end{bmatrix}
 * \f}
 *
 * @param w Angular velocity
 * @return The matrix \f$\boldsymbol{\Omega}\f$
 */
static inline Eigen::Matrix<double, 4, 4> Omega(Eigen::Matrix<double, 3, 1> w) {
    Eigen::Matrix<double, 4, 4> mat;
    mat.block(0, 0, 3, 3) = -skew_x(w);
    mat.block(3, 0, 1, 3) = -w.transpose();
    mat.block(0, 3, 3, 1) = w;
    mat(3, 3) = 0;
    return mat;
}

/**
 * @brief Computes left Jacobian of SO(3)
 *
 * The left Jacobian of SO(3) is defined equation (7.77b) in [State Estimation for
 * Robotics](http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17.pdf) by Timothy D. Barfoot @cite Barfoot2017. Specifically it is the
 * following (with \f$\theta=|\boldsymbol\theta|\f$ and \f$\mathbf a=\boldsymbol\theta/|\boldsymbol\theta|\f$): \f{align*}{
 * J_l(\boldsymbol\theta) = \frac{\sin\theta}{\theta}\mathbf I + \Big(1-\frac{\sin\theta}{\theta}\Big)\mathbf a \mathbf a^\top +
 * \frac{1-\cos\theta}{\theta}\lfloor \mathbf a \times\rfloor \f}
 *
 * @param w axis-angle 
 * @return The left Jacobian of SO(3)
 */
inline Eigen::Matrix<double, 3, 3> Jl_so3(const Eigen::Matrix<double, 3, 1> &w) {
    double theta = w.norm();
    if (theta < 1e-6) {
        return Eigen::MatrixXd::Identity(3, 3);
    } else {
        Eigen::Matrix<double, 3, 1> a = w / theta;
        Eigen::Matrix<double, 3, 3> J = sin(theta) / theta * Eigen::MatrixXd::Identity(3, 3) + (1 - sin(theta) / theta) * a * a.transpose() +
                                        ((1 - cos(theta)) / theta) * skew_x(a);
        return J;
    }
}

/**
 * @brief Computes right Jacobian of SO(3)
 *
 * The right Jacobian of SO(3) is related to the left by Jl(-w)=Jr(w).
 * See equation (7.87) in [State Estimation for Robotics](http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17.pdf) by Timothy D. Barfoot
 * @cite Barfoot2017. See @ref Jl_so3() for the definition of the left Jacobian of SO(3).
 *
 * @param w axis-angle
 * @return The right Jacobian of SO(3)
 */
inline Eigen::Matrix<double, 3, 3> Jr_so3(const Eigen::Matrix<double, 3, 1> &w) { return Jl_so3(-w); }

}

#endif